
#include <Aabb.h>
#include <Matrix4.h>
#include <Ray.h>
#include <DrawUtils.h>

#include <OpenGL/OpenGL.h>
#include <OpenGL/gl.h>
#include <OpenGL/glu.h>


void Aabb::reset()
{
	const float HUGEVAL = 1.e10f;
	m_maxima = Vec4(-HUGEVAL, -HUGEVAL, -HUGEVAL);
	m_minima = Vec4(HUGEVAL, HUGEVAL, HUGEVAL);
}


void Aabb::includePoint(const Vec4& point)
{
	for (int a=0; a<3; ++a)
	{
		if ( point[a] > m_maxima[a] ) m_maxima[a] = point[a];
		if ( point[a] < m_minima[a] ) m_minima[a] = point[a];
	}
}

void Aabb::compute( const vector<Vec4>& worldVerts )
{
	int nVerts = worldVerts.size();
	if (nVerts<1) return;
	
	m_maxima = worldVerts[0];
	m_minima = worldVerts[0];
	for (int n=1; n<nVerts; ++n)
	{
		const Vec4& verts = worldVerts[n];
		for (int a=0; a<3; ++a)
		{
			if ( verts[a] > m_maxima[a] ) m_maxima[a] = verts[a];
			if ( verts[a] < m_minima[a] ) m_minima[a] = verts[a];
		}
	}
}


void Aabb::add( const Aabb& aabbOther )
{
	const Vec4& maximaOther = aabbOther.m_maxima;
	const Vec4& minimaOther = aabbOther.m_minima;

	for (int a=0; a<3; ++a)
	{
		if ( maximaOther[a] > m_maxima[a] ) m_maxima[a] = maximaOther[a];
		if ( minimaOther[a] < m_minima[a] ) m_minima[a] = minimaOther[a];
	}
}


void Aabb::expand( float factor )
{
	Vec4 diff = (m_maxima - m_minima) * (0.5f * factor);
	Vec4 center = (m_maxima + m_minima) * 0.5f;

	m_maxima = center + diff;
	m_minima = center - diff;
}


void Aabb::transform( const Matrix4& transform )
{
	// original AABB points
	Vec4 vertex_orig[8];
	float xMin = m_minima[0]; float xMax = m_maxima[0];
	float yMin = m_minima[1]; float yMax = m_maxima[1];
	
	for (int i=0; i<2; ++i)
	{
		float z = (i==0) ? m_maxima[2] : m_minima[2];
		vertex_orig[4*i  ] = Vec4( xMax, yMax, z );
		vertex_orig[4*i+1] = Vec4( xMax, yMin, z );
		vertex_orig[4*i+2] = Vec4( xMin, yMax, z );
		vertex_orig[4*i+3] = Vec4( xMin, yMin, z );
	}

	//transform points
	vector<Vec4> vertex_trans; vertex_trans.resize(8);
	for (int n=0; n<8; ++n)
	{
		vertex_orig[n][3] = 1.0f;
		vertex_trans[n] = transform * vertex_orig[n];
	}
	
	//compute the AABB of the transformed points
	compute(vertex_trans);
}


float Aabb::surfaceArea( ) const
{
	Vec4 extents = m_maxima - m_minima;
	
	float A = 0.0f;
	for (int axis=0; axis<3; ++axis)
	{
		int a = (axis+1)%3;
		int b = (axis+2)%3;
		A += 2.0f * extents[a] * extents[b];
	}

	return A;
}


// length of the largest edge
float Aabb::size() const
{
	Vec4 diff = m_maxima - m_minima;
	float maxVal;
	diff.maxIndex(maxVal);
	return maxVal;
}
	


void Aabb::draw()
{
	Vec4 center      = (m_maxima + m_minima) * 0.5f;
	Vec4 halfExtents = (m_maxima - m_minima) * 0.5f;
	
	glPushMatrix();
	glTranslatef(center[0], center[1], center[2]);

	DrawUtils::drawBox(halfExtents);
	
	glPopMatrix();
}



static inline bool _computeFaceIntersection( int face, const float *planes, 
											const Vec4& o, const Vec4& n, float *t )
{
	int i = face/2;
	
	if ( n[i] == 0.0f ) return 0;
	
	float lambda = ( planes[face] - o[i] ) / n[i];
	
	int a_indx = (i+1) % 3;
	int b_indx = (i+2) % 3;
	
	float a = lambda * n[a_indx] + o[a_indx];
	float b = lambda * n[b_indx] + o[b_indx];
	
	bool aHit = ( a > planes[a_indx*2] ) && ( a < planes[a_indx*2+1] );
	bool bHit = ( b > planes[b_indx*2] ) && ( b < planes[b_indx*2+1] );
	
	if ( aHit && bHit )
	{	
		*t = lambda;
		return true;
	}
	
	return false;
}


bool Aabb::intersectRay( const Ray& R, float& t_entryOut, float& t_exitOut ) const
{
	float planes[6];
	for (int i=0; i<3; ++i)
	{
		planes[2*i]   = m_minima[i];
		planes[2*i+1] = m_maxima[i];
	}
	
	// 6 faces, indexed as: -X = 0, +X = 1, -Y = 2, +Y = 3, -Z = 4, +Z = 5 
	// note the index of the normal axis is given by face/2 (face = 0,..,5)
	bool hitface[6];
	
	// if there was a hit on a particular face, record the ray distance to that hit from the origin
	float hit_dist[6];
	
	for (int face=0; face<6; ++face) 
	{
		hitface[face] = _computeFaceIntersection( face, planes, R.m_origin, R.m_direction, &hit_dist[face] );
	}
	
	// If the box was hit, there must have been two face hits. Find which faces
	int hits = 0;
	int hit_faces[2];
	for (int face = 0; face<6; face++) 
	{
		if ( hitface[face] ) 
		{
			hit_faces[hits++] = face;
			if (hits>1) break;
		}
	}
	
	if (hits != 2)
	{
		// no intersection
		return false;
	}
	
	// given intersection distances, determine what happened
	float t0 = hit_dist[ hit_faces[0] ];
	float t1 = hit_dist[ hit_faces[1] ];
	
	if ( ( t0 < 0.0f ) && ( t1 < 0.0f ) )
	{
		//intersections behind start of ray
		return false;
	}
	
	if ( ( t0 > 0.0f ) && ( t1 > 0.0f ) )
	{
		//hit from exterior
		if ( t0 > t1 )
		{
			t_entryOut = t1;
			t_exitOut = t0;
		}
		else
		{
			t_exitOut = t1;
			t_entryOut = t0;
		}
	}
	else
	{
		// hit from interior
		if ( t0 >= 0.0f )
		{
			t_entryOut = t1;
			t_exitOut = t0;
		}
		else
		{
			t_exitOut = t1;
			t_entryOut = t0;
		}
	}
	
	return true;		
}



bool Aabb::overlaps(const Aabb& other)
{
	for (int a=0; a<3; ++a)
	{
		if ( m_minima[a] > other.m_maxima[a] ) return false;
		if ( m_maxima[a] < other.m_minima[a] ) return false;
	}
	return true;
}




